// Copyright 2008 Isis Innovation Limited
// This is the main extry point for PTAM
#include <stdlib.h>
#include <iostream>
#include <gvars3/instances.h>
#include <TooN/se3.h>
#include "System.h"
#include "RandomHelpers.h"
#include "SE3GaussianPDF.h"
#include "px.h"
#include "mavlink.h"
#include <math.h>

const float RADIANS_PER_DEGREE = M_PI/180.0;

using namespace std;
using namespace GVars3;
bool verbose;

/**
 * @brief Handle a MAVLINK message over LCM
 *
 * @param rbuf LCM receive buffer
 * @param channel LCM channel
 * @param msg MAVLINK message
 * @param user LCM user
 */
static void mavlink_handler(const lcm_recv_buf_t *rbuf, const char * channel, const mavlink_message_t* msg, void * user)
{
  const double imuTimeTolerance = 40000;
  const double gpsTimeTolerance = 100000;

  // World has x-axis east, y-axis north, z-axis up (UTM)
  // GPS has same axes as world
  // IMU has x-axis forward, y-axis left, z-axis up
  // Camera has x-axis as image plane right, y-axis as image plane down, z-axis outwards along optical axis

  static double offsetTime = 0;
  static double imgTime = 0;

  // Camera w.r.t IMU -> only orientation (orientation order is yaw->pitch->roll, +ve angles are anticlockwise when looking into axis)
  static double imuTime = 0;
  const SO3<> so3IMUfromC = PackSO3_RPY(0.0, M_PI, M_PI/2);
  const Vector<3> v3ImuMeasNoise = makeVector(0.1,0.1,0.1);
  static SO3<> so3GfromIMU;
  static Vector<3> v3AngularVelocity;

  // Camera w.r.t GPS -> only position
  static double gpsTime = 0;
  const Vector<3> v3GPSfromC = makeVector(0.0, 0.0, 0.0);
  static TooN::Vector<3> v3GpsMeasNoise;
  static TooN::Vector<3> v3GfromGPS;
  static TooN::Vector<2> v2HorizontalVelocity;

	switch(msg->msgid)
	{
	  case MAVLINK_MSG_ID_IMU_RAW_ASCTEC:
	  {
		  mavlink_imu_raw_asctec_t raw_imu;
      mavlink_msg_imu_raw_asctec_decode(msg, &raw_imu);

      imuTime = (double)raw_imu.usec;
      so3GfromIMU = PackSO3_RPY(raw_imu.roll, raw_imu.pitch, raw_imu.yaw);
      v3AngularVelocity = makeVector(raw_imu.rvel, raw_imu.pvel, raw_imu.yvel);

      if(verbose)
        std::cout << "IMU: " << (imuTime-offsetTime)/60000000 << " (" << raw_imu.roll << "," << raw_imu.pitch << "," << raw_imu.yaw << ")" << std::endl;
	  }
	  break;

	  case MAVLINK_MSG_ID_GPS_RAW_ASCTEC:
	  {
		  mavlink_gps_raw_asctec_t raw_gps;
      mavlink_msg_gps_raw_asctec_decode(msg, &raw_gps);

      if(raw_gps.status) // Check for a valid gps fix first (we get messages even if there is not)
      {
        gpsTime = (double)raw_gps.usec;
        v3GfromGPS = makeVector(raw_gps.east, raw_gps.north, raw_gps.alt);
        v2HorizontalVelocity = makeVector(raw_gps.vx, raw_gps.vy);
        v3GpsMeasNoise = makeVector(raw_gps.eph, raw_gps.eph, raw_gps.epv);

        if(verbose)
        {
          std::cout << "GPS: " << (gpsTime-offsetTime)/60000000 << " (" << raw_gps.east << "," << raw_gps.north << "," << raw_gps.alt << ")";
          std::cout << "[" << raw_gps.eph << "," << raw_gps.epv << "]" << std::endl;
        }
      }
	  }
	  break;

	  case MAVLINK_MSG_ID_IMAGE_AVAILABLE:
	  {
      bool bSyncGPS = false, bSyncIMU = false;
      if(imgTime == 0)
      {
        imgTime = (double)mavlink_msg_image_available_get_timestamp(msg);
        offsetTime = imgTime;
      }
      else
        imgTime = (double)mavlink_msg_image_available_get_timestamp(msg);

      SE3GaussianPDF se3PdfGfromC;

      if(fabs(imgTime-imuTime) < imuTimeTolerance)
      {
        bSyncIMU = true;
        Vector<3> v3AngularMotion = v3AngularVelocity * (1e-6 * (imgTime - imuTime));
        SO3<> so3AngularMotion = PackSO3_RPY(v3AngularMotion[0], v3AngularMotion[1], v3AngularMotion[2]);
        so3GfromIMU = so3GfromIMU * so3AngularMotion;
        SO3<> so3GfromC = so3GfromIMU * so3IMUfromC;

        if(fabs(imgTime-gpsTime) < gpsTimeTolerance)
        {
          bSyncGPS = true;
          Vector<2> v2HorizontalMotion = v2HorizontalVelocity * (1e-6 * (imgTime - gpsTime));
          v3GfromGPS.slice(0,2) = v3GfromGPS.slice(0,2) + v2HorizontalMotion;
          Vector<3> v3GfromC = v3GfromGPS + v3GPSfromC;
  
          se3PdfGfromC = SE3GaussianPDF(so3GfromC, v3ImuMeasNoise, v3GfromC, v3GpsMeasNoise);
        }
        else
        {
          se3PdfGfromC = SE3GaussianPDF(so3GfromC, v3ImuMeasNoise);
        }
      }

      if(verbose)
        std::cout << "IMG: " << (imgTime-offsetTime)/60000000 << " synched with GPS: " << bSyncGPS << ", IMU: " << bSyncIMU << std::endl;

	    System *s = static_cast<System*>(user);
      s->NewObservations(msg, se3PdfGfromC);
	  }
	  break;

	  default: //Ignore anything else
	  break;
	}
}


int main()
{
  cout << "  You are running SPTAM: " << endl;
  cout << "  Superior Parallel tracking and mapping for large workspaces" << endl;

  GUI.LoadFile("./config/settings.cfg");
  static gvar3<int> gvnVerbose("System.Verbose", 0, HIDDEN|SILENT);
  static gvar3<int> gvnUseGUI("System.UseGUI", 1, HIDDEN|SILENT);

  static gvar3<int> gvnGlobalSO3("System.GlobalSO3", 0, HIDDEN|SILENT); 
  if(*gvnGlobalSO3)
    cout << "GlobalSO3: YES" << endl;
  else
    cout << "GlobalSO3: NO"  << endl;

  static gvar3<int> gvnGlobalSE3("System.GlobalSE3", 0, HIDDEN|SILENT);
  if(*gvnGlobalSE3)
    cout << "GlobalSE3: YES" << endl;
  else
    cout << "GlobalSE3: NO"  << endl;

  if(*gvnUseGUI)
  {
    GUI.StartParserThread(); // Start parsing of the console input
    atexit(GUI.StopParserThread); 
  }

  if(*gvnVerbose)
    verbose = true;
  else
    verbose = false;

  lcm_t* lcm = lcm_create (NULL);

	if(!lcm)
		cout << "ERROR: Could not start LCM link." << endl;
	else
	{
    try
    {
      System s(*gvnUseGUI);
      mavlink_message_t_subscription_t *comm_sub = mavlink_message_t_subscribe(lcm, "MAVLINK", &mavlink_handler, &s);

	    while(!s.Stop())
	    {
		    // Block waiting for new messages, when a message is received it will be processed
		    lcm_handle(lcm);
	    }

	    mavlink_message_t_unsubscribe(lcm, comm_sub);
	    lcm_destroy(lcm);
	    exit(EXIT_SUCCESS);
    }
    catch(CVD::Exceptions::All e)
    {
      cout << "ERROR: Failed to run system, got exception: " << e.what << endl;
    }
  }
}

